clear;close all;clc;
dbstop if error;


data = '20251009';
mkdir("3DResults\"+data);%创建文件夹
numImages=1;%图片组数

Pro_width = 1280;
Pro_height = 720;

N = 20;T = 20;Plus =2.98451302091030;
f = Pro_width / T;
n=log2(f);%互补张数

width = 2048;
height = 1536;
% T = [];
% for i = 1:3
%     T(i) = width/f(i);
% end
P=20/2/pi;
Q=20/2/pi;
Theta=5;
%生成高斯滤波
GaussianFilter = fspecial('gaussian', [3,3], 3);%最后一个参数为sigma
N=20;



% for i=1:numImages
%     file_old = "Pic\"+num2str(data)+"\OldObjects\"+num2str(i);
%     file_new = "Pic\"+num2str(data)+"\Objects\"+num2str(i);
%     Rename(string(file_old),55 ,string(file_new));
% end

%%
%读取标定参数



cameraParams_IntrinsicMatrix=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.CameraParameters1.K;
Projector_IntrinsicMatrix=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.CameraParameters2.K;

Pro2Camera_R=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.PoseCamera2.R;
Pro2Camera_T=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.PoseCamera2.Translation';

Camera_radialDistortion=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.CameraParameters1.RadialDistortion;
Camera_tangentialDistortion=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.CameraParameters1.TangentialDistortion;
Pro_radialDistortion=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.CameraParameters2.RadialDistortion;
Pro_tangentialDistortion=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.CameraParameters2.TangentialDistortion;
FundamentalMatrix=load("../立体标定/SysCalibrtionResults/"+data+"/Sys_Result/stereoParams.mat").stereoParams.FundamentalMatrix;

M1=load("../立体标定/SysCalibrtionResults\"+data+"\Sys_Result"+'\M1.mat').M1;
M2=load("../立体标定/SysCalibrtionResults\"+data+"\Sys_Result"+'\M2.mat').M2;

Hc = cameraParams_IntrinsicMatrix * [[1,0,0;0,1,0;0,0,1], [0;0;0]];
Hp = Projector_IntrinsicMatrix * [Pro2Camera_R,Pro2Camera_T];
folder="Pic\"+data+"\Objects\";


%%
%解相位 得到其中一个方向的坐标
for index = 1:numImages
    tic
    %计算横条纹包裹相位
    H_I_sin = zeros(height, width);
    H_I_cos = zeros(height, width);
    H_I_B = zeros(height, width);
    H_I_back = zeros(height, width);
    disp("H二值相移图");
    for k = 1: N
        % 计算当前相位移动的值
        shiftVal = (2 * pi / N) * (k-1);% 普通相移值
        % 根据相位值计算当前像素的强度
        I = im2double(imread(folder+num2str(index)+"\"+num2str((n+1)+(k))+".bmp"));
        disp(folder+num2str(index)+"\"+num2str((n+1)+(k))+".bmp");
        H_I_sin = H_I_sin + I .* sin(shiftVal);
        H_I_cos = H_I_cos + I .* cos(shiftVal);
        H_I_back=H_I_back+I.*255;
    end
    H_phase = -atan2(H_I_sin, H_I_cos); % 计算截断相位
    H_I_B = 2.0/N.*sqrt(H_I_sin.^2+H_I_cos.^2).*255;
    H_I_back=H_I_back./N;
    H_ShadowMask=H_I_B>5;
    H_ShadowMask = bwareaopen(H_ShadowMask,100);
    
    
    %计算条纹阶次

    H1 = zeros(height, width);
    for k = 1: n
        I = imread(folder+num2str(index)+"\"+num2str(k)+".bmp");
        %disp(folder+num2str(index)+"\"+num2str(k)+".bmp");
        I = (I>H_I_back).*H_ShadowMask;
        H1 = H1+double(I).*2^(n-k);
    end
    disp("H格雷码解码优化版本");

    H_BinarybitPlane = cell(n+1, 1);
    H_FloatBinarybitPlane = cell(n+1, 1);

    H_powersV2 = zeros(1, n+1);
    H_powersV1 = zeros(1, n);

    H_powersV2(1) = 2^n;
    for k = 2:(n+1)
        H_powersV2(k) = H_powersV2(k-1) / 2.0;
    end

    for k = 1:n
        H_powersV1(k) = H_powersV2(k+1);
    end

    for k = 1:(n+1)
        I = imread(folder+num2str(index)+"\"+num2str(k)+".bmp");
        if k <= n
            disp(folder+num2str(index)+"\"+num2str(k)+".bmp");
        else
            disp("H互补格雷码");
            disp(folder+num2str(index)+"\"+num2str(k)+".bmp");
            imshow((I>H_I_back).*H_ShadowMask);
        end

        BinaryI = (I > H_I_back) .* H_ShadowMask;
        H_BinarybitPlane{k} = BinaryI;

        if k > 1
            H_BinarybitPlane{k} = xor(H_BinarybitPlane{k-1}, H_BinarybitPlane{k});
        end

        H_FloatBinarybitPlane{k} = double(H_BinarybitPlane{k});
    end

    HK1 = zeros(height, width);
    HK2 = zeros(height, width);

    for k = 1:(n+1)
        if k <= n
            HK1 = HK1 + H_FloatBinarybitPlane{k} * H_powersV1(k);
        end
        HK2 = HK2 + H_FloatBinarybitPlane{k} * H_powersV2(k);
    end

    HK2 = floor((HK2 + 1.0) / 2.0);

    H_mask1 = double(H_phase <= -pi / 2.);
    H_mask2 = double((-pi / 2 < H_phase) & (H_phase <= pi / 2.));
    H_mask3 = double(H_phase >  pi / 2.);
    PHDeformed_Unwrap = (H_phase + 2. * pi * HK2) .* H_mask1 + (H_phase + 2. * pi * HK1) .* H_mask2 + (H_phase + 2. * pi * HK2 - 2. * pi) .* H_mask3;
    PHDeformed_Unwrap=PHDeformed_Unwrap+Plus;
    PHDeformed_Unwrap(H_ShadowMask==0) = nan;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




    %计算横条纹包裹相位
    V_I_sin = zeros(height, width);
    V_I_cos = zeros(height, width);
    V_I_B = zeros(height, width);
    V_I_back = zeros(height, width);
    disp("V二值相移图");
    for k = 1: N
        % 计算当前相位移动的值
        shiftVal = (2 * pi / N) * (k-1);% 普通相移值
        % 根据相位值计算当前像素的强度
        I = im2double(imread(folder+num2str(index)+"\"+num2str((n+1)+N+(n+1)+(k))+".bmp"));
        disp(folder+num2str(index)+"\"+num2str((n+1)+N+(n+1)+(k))+".bmp");
        V_I_sin = V_I_sin + I .* sin(shiftVal);
        V_I_cos = V_I_cos + I .* cos(shiftVal);
        V_I_back=V_I_back+I.*255;
    end
    V_phase = -atan2(V_I_sin, V_I_cos); % 计算截断相位
    V_I_B = 2.0/N.*sqrt(V_I_sin.^2+V_I_cos.^2).*255;
    V_I_back=V_I_back./N;
    V_ShadowMask=V_I_B>10;
    V_ShadowMask = bwareaopen(V_ShadowMask,100);

    V_BinarybitPlane = cell(n+1, 1);
    V_FloatBinarybitPlane = cell(n+1, 1);
    
    V_powersV2 = zeros(1, n+1);
    V_powersV1 = zeros(1, n);
    
    V_powersV2(1) = 2^n;
    for k = 2:(n+1)
        V_powersV2(k) = V_powersV2(k-1) / 2.0;
    end
    
    for k = 1:n
        V_powersV1(k) = V_powersV2(k+1);
    end
    
    for k = 1:(n+1)
        I = imread(folder+num2str(index)+"\"+num2str((n+1)+N+k)+".bmp");
        if k <= n
            disp(folder+num2str(index)+"\"+num2str((n+1)+N+k)+".bmp");
        else
            disp("V互补格雷码");
            disp(folder+num2str(index)+"\"+num2str((n+1)+N+k)+".bmp");
            imshow((I>V_I_back).*V_ShadowMask);
        end
        
        BinaryI = (I > V_I_back) .* V_ShadowMask;
        V_BinarybitPlane{k} = BinaryI;
        
        if k > 1
            V_BinarybitPlane{k} = xor(V_BinarybitPlane{k-1}, V_BinarybitPlane{k});
        end
        
        V_FloatBinarybitPlane{k} = double(V_BinarybitPlane{k});
    end
    
    VK1 = zeros(height, width);
    VK2 = zeros(height, width);
    
    for k = 1:(n+1)
        if k <= n
            VK1 = VK1 + V_FloatBinarybitPlane{k} * V_powersV1(k);
        end
        VK2 = VK2 + V_FloatBinarybitPlane{k} * V_powersV2(k);
    end
    
    VK2 = floor((VK2 + 1.0) / 2.0);

%%

    Vmask1 = double(V_phase <= -pi / 2.);
    Vmask2 = double((-pi / 2 < V_phase) & (V_phase <= pi / 2.));
    Vmask3 = double(V_phase >  pi / 2.);
    PVDeformed_Unwrap = (V_phase + 2. * pi * VK2) .* Vmask1 + (V_phase + 2. * pi * VK1) .* Vmask2 + (V_phase + 2. * pi * VK2 - 2. * pi) .* Vmask3;
    PVDeformed_Unwrap=PVDeformed_Unwrap+Plus;
    PVDeformed_Unwrap(V_ShadowMask==0)=nan;

    y_p=PHDeformed_Unwrap.*Q;
    x_p=PVDeformed_Unwrap.*P;

    %相机坐标
    [Xc,Yc]=meshgrid(0:width-1,0:height-1);
    %%
    % 三维重建公式列出
    % 计算矩阵中1的个数
    ones_count = sum(V_ShadowMask(:) == 1);
    XYZ_Save_Undistorted= zeros(ones_count,6);
    XYZ_Save_distorted = zeros(ones_count,3);
    XYZ_Save_undistorted_EpipolarConstraint= zeros(ones_count,3);
    XYZ_Save_undistorted_DoubleEpipolarConstraint= zeros(ones_count,6);

    mkdir("3DResults\"+data+"\Object");%创建文件夹

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %竖条纹重建(无去畸变)
    % x_rec = nan(height, width);
    % y_rec = nan(height, width);
    % z_rec = nan(height, width);
    % k=1;
    % for y = 1:height
    %     for x = 1:width
    %         if Mask(y, x)==1
    %             A = [Hc(1,1)-Hc(3,1)*Xc(y,x), Hc(1,2)-Hc(3,2)*Xc(y,x), Hc(1,3)-Hc(3,3)*Xc(y,x);
    %                 Hc(2,1)-Hc(3,1)*Yc(y,x), Hc(2,2)-Hc(3,2)*Yc(y,x), Hc(2,3)-Hc(3,3)*Yc(y,x);
    %                 Hp(1,1)-Hp(3,1)*(x_p(y,x)), Hp(1,2)-Hp(3,2)*(x_p(y,x)), Hp(1,3)-Hp(3,3)*(x_p(y,x))]; % Eq. (32) in the reference paper.
    %             b = [Hc(3,4)*Xc(y,x) - Hc(1,4); Hc(3,4)*Yc(y,x) - Hc(2,4); Hp(3,4)*(x_p(y,x)) - Hp(1,4)];
    %             X = A\b;
    %             x_rec(y, x) = X(1);
    %             y_rec(y, x) = X(2);
    %             z_rec(y, x) = X(3);
    %             XYZ_Save_distorted(k,:)=X';
    %             k=k+1;
    %         end
    %
    %         % if ~isnan(PVDeformed_Unwrap(y, x))
    %         %        P = [1; Xc(y,x); (x_p(y,x)-1); Yc(y,x);Xc(y,x).*(x_p(y,x)-1); (x_p(y,x)-1).*Yc(y,x);];
    %         %        S = [M2*P; M2*P; M2*P];
    %         %        T =  (M1*P)./ S;
    %         %        x_rec(y, x) = -T(1);
    %         %        y_rec(y, x) =  T(2);
    %         %        z_rec(y, x) = -T(3);
    %         %        XYZ_Save(end+1) = [x_rec(y, x),y_rec(y, x),z_rec(y, x)];
    %         % end
    %
    %
    %     end
    % end
    % save("3DResults\"+data+"\Object\Undistorted_"+num2str(index)+".txt", 'XYZ_Save_distorted', '-ascii');
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % %竖条纹 极线约束重建(去畸变)
    XcYc =UndistortedPoints([Xc(:),Yc(:)], cameraParams_IntrinsicMatrix, Camera_radialDistortion, Camera_tangentialDistortion) ;
    Xc= reshape(XcYc(:,1),[height, width]);
    Yc= reshape(XcYc(:,2),[height, width]);


    y_p = Epipolar_Curve(FundamentalMatrix,Xc,Yc,x_p);
    x_p_y_p =UndistortedPoints([x_p(:),y_p(:)], Projector_IntrinsicMatrix, Pro_radialDistortion, Pro_tangentialDistortion) ;
    x_p= reshape(x_p_y_p(:,1),[height, width]);
    y_p= reshape(x_p_y_p(:,2),[height, width]);

    x_rec = nan(height, width);
    y_rec = nan(height, width);
    z_rec = nan(height, width);
    k=1;
    tic
    % for y = 1:height
    %     for x = 1:width
    %         if V_ShadowMask(y, x)==1
    %             A = [Hc(1,1)-Hc(3,1)*Xc(y,x), Hc(1,2)-Hc(3,2)*Xc(y,x), Hc(1,3)-Hc(3,3)*Xc(y,x);
    %                 Hc(2,1)-Hc(3,1)*Yc(y,x), Hc(2,2)-Hc(3,2)*Yc(y,x), Hc(2,3)-Hc(3,3)*Yc(y,x);
    %                 Hp(1,1)-Hp(3,1)*(x_p(y,x)), Hp(1,2)-Hp(3,2)*(x_p(y,x)), Hp(1,3)-Hp(3,3)*(x_p(y,x))]; % Eq. (32) in the reference paper.
    %             b = [Hc(3,4)*Xc(y,x) - Hc(1,4); Hc(3,4)*Yc(y,x) - Hc(2,4); Hp(3,4)*(x_p(y,x)) - Hp(1,4)];
    %             X = A\b;
    %             x_rec(y, x) = X(1);
    %             y_rec(y, x) = X(2);
    %             z_rec(y, x) = X(3);
    %             % XYZ_Save_undistorted_EpipolarConstraint(k,:)=[X',color(y,x),color(y,x),color(y,x)];
    %
    %             XYZ_Save_distorted(k,:)=X';
    %             k=k+1;
    %         end
    % end

    for y = 1:height
        for x = 1:width
            if V_ShadowMask(y, x)==1
                G = [1; Xc(y,x); (x_p(y,x)); Yc(y,x);Xc(y,x).*(x_p(y,x)); (x_p(y,x)).*Yc(y,x);];
                S = [M2*G; M2*G; M2*G];
                T =  (M1*G)./ S;
                x_rec(y, x) = -T(1);
                y_rec(y, x) =  T(2);
                z_rec(y, x) = -T(3);
                XYZ_Save_undistorted_EpipolarConstraint(k,:)= [x_rec(y, x),y_rec(y, x),z_rec(y, x)];
                % XYZ_Save_undistorted_EpipolarConstraint(k,1:6) = [x_rec(y, x),y_rec(y, x),z_rec(y, x),color(y,x),color(y,x),color(y,x)];
                k=k+1;
            end


        end
    end
    toc

    save("3DResults\"+data+"\Object\Undistorted_EpipolarConstraint_"+num2str(index)+".txt", 'XYZ_Save_undistorted_EpipolarConstraint', '-ascii');
    % ptCloud=pointCloud(XYZ_Save_undistorted_EpipolarConstraint(:,1:3),'Color',XYZ_Save_undistorted_EpipolarConstraint(:,4:6));
    % pcwrite(ptCloud,"3DResults\"+data+"\Object\Undistorted_EpipolarConstraint_"+num2str(index)+".ply");
    % save("3DResults\"+data+"\Object\Undistorted_EpipolarConstraint_"+num2str(index)+".txt", 'XYZ_Save_distorted', '-ascii');



    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %竖条纹双二次极线约束三维重建
    %         XcYc =UndistortedPoints([Xc(:),Yc(:)], cameraParams_IntrinsicMatrix, Camera_radialDistortion, Camera_tangentialDistortion) ;
    %         Xc= reshape(XcYc(:,1),[height, width]);
    %         Yc= reshape(XcYc(:,2),[height, width]);
    %
    %
    %         [~,y_p] = DoubleEpipolar_Curve(FundamentalMatrix,Projector_IntrinsicMatrix, Pro_radialDistortion, Pro_tangentialDistortion,Xc,Yc,x_p);
    %
    %
    %         x_rec = nan(height, width);
    %         y_rec = nan(height, width);
    %         z_rec = nan(height, width);
    %         k=1;
    %          for y = 1:height
    %             for x = 1:width
    %                 if Mask(y, x)==1
    %                     A = [Hc(1,1)-Hc(3,1)*Xc(y,x), Hc(1,2)-Hc(3,2)*Xc(y,x), Hc(1,3)-Hc(3,3)*Xc(y,x);
    %                         Hc(2,1)-Hc(3,1)*Yc(y,x), Hc(2,2)-Hc(3,2)*Yc(y,x), Hc(2,3)-Hc(3,3)*Yc(y,x);
    %                         Hp(2,1)-Hp(3,1)*(y_p(y,x)), Hp(2,2)-Hp(3,2)*(y_p(y,x)), Hp(2,3)-Hp(3,3)*(y_p(y,x))]; % Eq. (32) in the reference paper.
    %                     b = [Hc(3,4)*Xc(y,x) - Hc(1,4); Hc(3,4)*Yc(y,x) - Hc(2,4); Hp(3,4)*(y_p(y,x)) - Hp(2,4)];
    %                     X = A\b;
    %                     x_rec(y, x) = X(1);
    %                     y_rec(y, x) = X(2);
    %                     z_rec(y, x) = X(3);
    %                     XYZ_Save_undistorted_EpipolarConstraint(k,:)=[X',,color(y,x),color(y,x),color(y,x)]];
    %                     k=k+1;
    %                 end
    %
    %                 % if ~isnan(PVDeformed_Unwrap(y, x))
    %                 %        P = [1; Xc(y,x); (x_p(y,x)-1); Yc(y,x);Xc(y,x).*(x_p(y,x)-1); (x_p(y,x)-1).*Yc(y,x);];
    %                 %        S = [M2*P; M2*P; M2*P];
    %                 %        T =  (M1*P)./ S;
    %                 %        x_rec(y, x) = -T(1);
    %                 %        y_rec(y, x) =  T(2);
    %                 %        z_rec(y, x) = -T(3);
    %                 %        XYZ_Save(end+1) = [x_rec(y, x),y_rec(y, x),z_rec(y, x)];
    %                 % end
    %
    %
    %             end
    %         end
    % %
    % %
    % %
    % ptCloud=pointCloud(XYZ_Save_undistorted_EpipolarConstraint(:,1:3),'Color',XYZ_Save_undistorted_EpipolarConstraint(:,4:6));
    % pcwrite(ptCloud,"3DResults\"+data+"\Object\Undistorted_DoubleEpipolarConstraint_"+num2str(index)+".ply");
    % save("3DResults\"+data+"\Object\Undistorted_DoubleEpipolarConstraint_"+num2str(index)+".txt", 'XYZ_Save_undistorted_EpipolarConstraint', '-ascii');
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % %横竖条纹三维重建
    %         XcYc =UndistortedPoints([Xc(:),Yc(:)], cameraParams_IntrinsicMatrix, Camera_radialDistortion, Camera_tangentialDistortion) ;
    %         Xc= reshape(XcYc(:,1),[height, width]);
    %         Yc= reshape(XcYc(:,2),[height, width]);
    %         x_p_y_p =UndistortedPoints([x_p(:),y_p(:)], Projector_IntrinsicMatrix, Pro_radialDistortion, Pro_tangentialDistortion) ;
    %         x_p= reshape(x_p_y_p(:,1),[height, width]);
    %         y_p= reshape(x_p_y_p(:,2),[height, width]);
    %
    %         %竖条纹重建(去畸变)
    %         % 3D reconstruction
    %         x_rec = nan(height, width);
    %         y_rec = nan(height, width);
    %         z_rec = nan(height, width);
    %         k=1;
    %         for y = 1:height
    %             for x = 1:width
    %                 if Mask(y, x)==1
    %                     A = [Hc(1,1)-Hc(3,1)*Xc(y,x), Hc(1,2)-Hc(3,2)*Xc(y,x), Hc(1,3)-Hc(3,3)*Xc(y,x);
    %                         Hc(2,1)-Hc(3,1)*Yc(y,x), Hc(2,2)-Hc(3,2)*Yc(y,x), Hc(2,3)-Hc(3,3)*Yc(y,x);
    %                         Hp(1,1)-Hp(3,1)*(x_p(y,x)), Hp(1,2)-Hp(3,2)*(x_p(y,x)), Hp(1,3)-Hp(3,3)*(x_p(y,x))]; % Eq. (32) in the reference paper.
    %                     b = [Hc(3,4)*Xc(y,x) - Hc(1,4); Hc(3,4)*Yc(y,x) - Hc(2,4); Hp(3,4)*(x_p(y,x)) - Hp(1,4)];
    %                     X = A\b;
    %                     x_rec(y, x) = X(1);
    %                     y_rec(y, x) = X(2);
    %                     z_rec(y, x) = X(3);
    %                     XYZ_Save_Undistorted(k,:)=[X',color(y,x),color(y,x),color(y,x)]];
    %                     k=k+1;
    %                 end
    %
    %                 % if ~isnan(PVDeformed_Unwrap(y, x))
    %                 %        P = [1; Xc(y,x); (x_p(y,x)-1); Yc(y,x);Xc(y,x).*(x_p(y,x)-1); (x_p(y,x)-1).*Yc(y,x);];
    %                 %        S = [M2*P; M2*P; M2*P];
    %                 %        T =  (M1*P)./ S;
    %                 %        x_rec(y, x) = -T(1);
    %                 %        y_rec(y, x) =  T(2);
    %                 %        z_rec(y, x) = -T(3);
    %                 %        XYZ_Save(end+1) = [x_rec(y, x),y_rec(y, x),z_rec(y, x)];
    %                 % end
    %
    %
    %             end
    %         end
    % ptCloud=pointCloud(XYZ_Save_Undistorted(:,1:3),'Color',XYZ_Save_Undistorted(:,4:6));
    % pcwrite(ptCloud,"3DResults\"+data+"\Object\distorted_"+num2str(index)+".ply");
    % save("3DResults\"+data+"\Object\distorted_"+num2str(index)+".txt", 'XYZ_Save_distorted', '-ascii');
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end









